/*
 * Control.cpp
 *
 *  Created on: Apr 22, 2010
 *      Author: Soheil Damangir
 */

#include "Control.h"
#include "Sensors.h"
#include "../Util/Constants.h"
#include "../Vision/VisionManager.h"

namespace Group5
{

Robot::Robot(int right_index, int left_index, const Decision&_callback):
right("RightMotor", right_index), left("LeftMotor", left_index), callback(_callback)
{}

bool Robot::checkOccasion()
{
  occ.bFront = dist_to_front < Constants::get()->MIN_DIST;

  occ.jRight = dist_to_right > Constants::get()->MAX_MISS_A;
  occ.jLeft = dist_to_left > Constants::get()->MAX_MISS_A;

  occ.ball = VisionManager::getInstance()->getBallResult().detected;
  occ.bomb = VisionManager::getInstance()->getBombResult().detected;

  return occ.any();
}

void Robot::TaskCode()
{
  /*
   * A simple P controller for the robot to maintain at the middle of the corridor.
   */
  calc_distances();

  right.setSpeed(right_speed + Constants::get()->ALIGN_KP*(dist_to_right - mean_dist));
  left.setSpeed(left_speed + Constants::get()->ALIGN_KP*(dist_to_left - mean_dist));

  right_speed = right.getSpeed();
  left_speed = right.getSpeed();

  /*
   * If any special occasion i.e. junction, bomb etc. occurs decision callback will be raised.
   */
  if (checkOccasion())
    callback.onDecide(occ, *this);

  WaitPeriod();
}

void Robot::calc_distances()
{
  dist_to_right = (Sensors::ir_right_back.get_distance() + Sensors::ir_right_front.get_distance()) / 2;
  dist_to_left = (Sensors::ir_left_back.get_distance() + Sensors::ir_left_front.get_distance()) / 2;
  mean_dist = (dist_to_left+dist_to_right)/2;
  dist_to_front = Sensors::ir_front.get_distance();

  if (Sensors::ir_right_front.get_distance() > Sensors::ir_right_back.get_distance())
    right_slope = Sensors::ir_right_front.get_distance() / (Sensors::ir_right_back.get_distance() + 1);
  else
    right_slope = Sensors::ir_right_back.get_distance() / (Sensors::ir_right_front.get_distance() + 1);

  if (Sensors::ir_left_front.get_distance() > Sensors::ir_left_back.get_distance())
    left_slope = Sensors::ir_left_front.get_distance() / (Sensors::ir_left_back.get_distance() + 1);
  else
    left_slope = Sensors::ir_left_back.get_distance() / (Sensors::ir_left_front.get_distance() + 1);
}

int Robot::speed()
{
  return (right.getSpeed() + left.getSpeed()) / 2;
}
int Robot::omega()
{
  return 90 * (right.getSpeed() - left.getSpeed()) / Constants::get()->ROBOT_H_PER;
}
int Robot::dist()
{
  return (right.getDist() + left.getDist()) / 2;
}
int Robot::rot()
{
  return 90 * (right.getDist() - left.getDist()) / Constants::get()->ROBOT_H_PER;
}
void Robot::resetPos()
{
  right.resetDist();
  left.resetDist();
}
void Robot::stop()
{
  right.setSpeed(0);
  left.setSpeed(0);
  while (right.getSpeed() == 0 && left.getSpeed() == 0);
}

void Robot::turn_left()
{
  stop();
  resetPos();
  right.setSpeed(Constants::get()->VELOCITY);
  left.setSpeed(-Constants::get()->VELOCITY);
  while (rot() < 90);
  resetPos();
}
void Robot::turn_right()
{
  stop();
  resetPos();
  right.setSpeed(-Constants::get()->VELOCITY);
  left.setSpeed(Constants::get()->VELOCITY);
  while (rot() > -90);
  resetPos();
}
void Robot::u_turn()
{
  stop();
  resetPos();
  right.setSpeed(Constants::get()->VELOCITY);
  left.setSpeed(-Constants::get()->VELOCITY);
  while (rot() < 180);
  resetPos();
}
bool Robot::move_mm(int d, bool slow = false)
{
  resetPos();
  sgn = d > 0?1:-1;
  if (!slow)
  {
    right.setSpeed(sgn*Constants::get()->VELOCITY);
    left.setSpeed(sgn*Constants::get()->VELOCITY);
  }else{
    right.setSpeed(sgn*Constants::get()->VELOCITY_LOW);
    left.setSpeed(sgn*Constants::get()->VELOCITY_LOW);
  }
  while (dist() < d){
    calc_distances();
    checkOccasion();
    if (occ.bFront || occ.bomb)
    {
      stop();
      move_mm(-dist(), slow);
      resetPos();
      return false;
    }
  }
  resetPos();
  return true;
}
}
